/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once 
class BALProblem{
public:
    explicit BALProblem(const std::string &filename, bool use_quaternions=false);
    
    ~BALProblem(){
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }
    
    //save results to text file
    void WriteToFile(const std::string &filename) const;
    
    //save results to ply pointcloud
    void WriteToPLYFile(const std::string &filename) const;
    
    void Perturb(const double rotation_sigma,
                 const double translation_sigma,
                 const double point_sigma);
    
    int camera_block_size() const{ return use_quaternions_?10:9;}
    
    int point_block_size() const{ return 3; }
    
    int num_cameras() const {return num_cameras_; }
    
    int num_points() const {return num_points_; }
    
    int num_observations() const { return num_observations_; }
    
    int num_parameters() const { return num_parameters_; }
    
    const int *point_index( ) const { return point_index_; }
    
    const int *camera_index() const { return camera_index_; }
    
    const double *observations() const { return observations_; }
    
    const double *paramaters() const { return paramaters_; }
    
    const double *cameras() const{ return cameras_; }
    
    const double *points() const{ return paramaters_+camera_block_size()*num_cameras_;}
    
    //camera parameters the first addition
    double *mutable_cameras() {return parameters_; }
    
    double *mutable_points() {return paramaters_+camera_block_size()*num_cameras_;}
    
    double *mutable_camera_for_observation(int i){
        return mutable_cameras()+camera_index_[i]*camera_block_size();
    }
    
    double *mutable_point_for_observation(int i){        
        return mutable_points()+point_index_[i]*point_block_size();
    }
    
    const double *camera_for_observation(int i) const{
        return cameras()+camera_index_[i]*camera_block_size();
    }
    
    const double *points_for_observation(int i) const {
        return points()+point_index_[i]*point_block_size();
    }
    
private:
    void CameraToAngelAndCenter(const double *camera,
                                double *angle_axis,
                                double *canter) const;
                                
    void AngleAxisAndCenterToCamera(const double *angle_axis,
                                    const double *center,
                                    double *camera) const;
                                    
    int num_camreas_;
    int num_points_;
    int num_observations_;
    int num_parameters_;
    bool use_quaternions_;
    
    int *point_index_;  //each observation coupled the point index;
    int *camrea_index_; // each observation coupled the camera index
    double *observations_;
    double *parameters_;
};

    
    
    
    
    
    
    
